#include "rc.h"

using namespace NXMFC;

#define RC_MINCHECK 1100
#define RC_MAXCHECK 1900

const uint8_t stick_min_flag[4] = {1 << 0, 1 << 2, 1 << 4, 1 << 6};
const uint8_t stick_max_flag[4] = {1 << 1, 1 << 3, 1 << 5, 1 << 7};

rc::rc()
{
    stick_flag = 0;
    rc_delay = 0;
}

uint32_t rc::checkStick(uint16_t rol, uint16_t pit, uint16_t thr, uint16_t yaw)
{
    uint16_t Data[4] = {rol, pit, yaw, thr};
    uint32_t ret = 0;

	for (int i = 0; i < 4; i++)
	{
		if (Data[i] < 900 || Data[i] > 2100) //如果摇杆值不在正常范围内，则退出检查
			break;

		if (Data[i] < RC_MINCHECK)
			stick_flag |= stick_min_flag[i]; // check for MIN
		else
			stick_flag &= ~stick_min_flag[i];

		if (Data[i] > RC_MAXCHECK)
			stick_flag |= stick_max_flag[i]; // check for MAX
		else
			stick_flag &= ~stick_max_flag[i]; // check for MAX
	}

	if (stick_flag & 0xff) //如果任一摇杆在最大或最小位置
	{
		if (rc_delay < 200)
			rc_delay ++;
	}
	else
	{
		rc_delay = 0;
		stick_flag &= 0;
	}

	if (rc_delay == 5)
	{
        ret = stick_flag;
		stick_flag &= 0;
		rc_delay = 0;
	}

    return ret;
}

bool rc::isLeftStickBottomRight(uint32_t flags)
{
    return ((flags & YAW_H) && (flags & THR_L));
}

bool rc::isLeftStickBottomLeft(uint32_t flags)
{
    return ((flags & YAW_L) && (flags & THR_L));
}

bool rc::isRightStickBottomLeft(uint32_t flags)
{
    return ((flags & PIT_L) && (flags & ROL_L));
}
